/* improve_div.c */

#include "improve_div.h"
#include <memory.h>
#ifndef INT_MAX
#include <limits.h>
#endif

void compute_score(elem** B_g, elem* s, elem* k_g, elem M, elem* score,
				   int size, int last_removed_index)
{
	int i = 0;
	elem res = 0, *x = NULL;

	if (last_removed_index == -1)
	{
		x = (elem*)calloc(size, sizeof(elem));
		assert(x != NULL);

		mult_matrix_vector(x, B_g, s, size);

		for (i = 0; i < size; ++i)
		{
			score[i] = -2*(s[i]*x[i] + (k_g[i]*k_g[i]/M));
		}
		free(x);
	}
	else
	{
		for (i = 0; i < size; ++i)
		{
            if(score[i] == INT_MIN)
            {
                continue;
            }
                     
			if (i == last_removed_index )
			{
				score[i] = -score[i];
			}
			else
			{
				score[i] -= 
					4*s[i]*s[last_removed_index]*B_g[i][last_removed_index];
			}
		}
 	}
}

node* create_unmoved(int size, node** last_unmoved)
{
	node *unmoved = NULL;
	node *new_node = NULL;
	node *last_node = NULL;
	int i = 0;
	
	for (i = 0; i < size; ++i)
	{
		new_node = (node*)malloc(sizeof(node));
		new_node->index = i;
		new_node->was_moved = 0;
		new_node->next = NULL;

		if (i == 0)
		{
			unmoved = new_node;
			last_node = new_node;
		}
		else
		{
			last_node->next = new_node;
			last_node = new_node;
		}
	}

	*last_unmoved = last_node;
	return unmoved;
}

void init_unmoved(node* unmoved)
{
	node* curr_node = unmoved;
	int i = 0;

	while(curr_node != NULL)
	{
		curr_node->index = i;
		curr_node->was_moved = 0;
		curr_node = curr_node->next;

		++i;
	}
}

int calculate_max_score_index(elem* score, int size, node* unmoved)
{
	int i = 0, max_i = 0;
	elem max = INT_MIN;
	node *curr_node = unmoved;

	while(curr_node != NULL && !curr_node->was_moved)
	{
		if (score[curr_node->index] >= max)
		{
			max = score[curr_node->index];
			max_i = curr_node->index;
		}

		curr_node = curr_node->next;
	}
	return max_i;
}

int calculate_max_index(elem* score, int size)
{
	int i = 0, max_i = 0;
	elem max = INT_MIN;

	for (i = 0; i < size; ++i)
	{
		if (score[i] >= max)
		{
			max = score[i];
			max_i = i;
		}
	}
	return max_i;
}

void remove_from_nodes(node** unmoved, int max_score_index,
					   node** last_unmoved_node)
{
	node* curr_node = *unmoved;
	node* temp = NULL;

	/* Check we need to remove the first one */
	if (curr_node->index == max_score_index)
	{
		*unmoved = (*unmoved)->next;
		curr_node->was_moved = 1;
		curr_node->next = NULL;
		(*last_unmoved_node)->next = curr_node;
		*last_unmoved_node = curr_node;

		return;
	}

	while(curr_node->next != NULL)
	{
		if (curr_node->next->index == max_score_index)
		{
			/* Found the node to remove */
			temp = curr_node->next;
			temp->was_moved = 1;
			if (*last_unmoved_node != temp)
			{
				curr_node->next = curr_node->next->next;
				temp->next = NULL;
				(*last_unmoved_node)->next = temp;
				*last_unmoved_node = temp;
			}
			return;
		}

		curr_node = curr_node->next;
	}
	/* Didn't find the node... Should happen only during debug.. */
	assert(-1);
}

elem improve_2_div(elem** B_g, elem* s, elem* k_g, elem M, int size)
{
	elem delta_Q = 0, all_delta = 0;
	int i = 0, j = 0;

	int *indices = NULL;
	elem *score = NULL;
	elem *improve = NULL;
	node *unmoved = NULL, *last_unmoved = NULL;
	int max_score_index = -1, max_improvement_index = -1;

	indices = (int*)calloc(size, sizeof(int));
	assert(indices != NULL);

	improve = (elem*)calloc(size, sizeof(elem));
	assert(improve != NULL);

    /* Score is initialized to 0.
     * Whenever a member is set to INT_MIN, it won't participate in score 
     * calculations anymore
     */
	score = (elem*)calloc(size, sizeof(elem));
	assert(score != NULL);

	unmoved = create_unmoved(size, &last_unmoved);

	do
	{
		max_score_index = -1;
		init_unmoved(unmoved);

		memset(indices, 0, size*sizeof(int));
		memset(improve, 0, size*sizeof(elem));
		memset(score, 0, size*sizeof(elem));
		
		for (i = 0; i < size; ++i)
		{
			compute_score(B_g, s, k_g, M, score, size, max_score_index);

			max_score_index = calculate_max_score_index(score, size, unmoved);
 
			s[max_score_index] = -s[max_score_index];
			indices[i] = max_score_index;

			if (i == 0)
			{
				improve[i] = score[max_score_index];
			}
			else
			{
				improve[i] = improve[i - 1] + score[max_score_index];
			}

			remove_from_nodes(&unmoved, max_score_index, &last_unmoved);
            score[max_score_index] = INT_MIN;
		}
 
		max_improvement_index = calculate_max_index(improve, size);
		i = size - 1;
		while(i > max_improvement_index)
		{
			j = indices[i];
			s[j] = -s[j];
			i--;
		}

		if (max_improvement_index == size - 1)
		{
			delta_Q = 0;
		}
		else
		{
			delta_Q = improve[max_improvement_index];
		}
		all_delta += delta_Q;
	}
	while(IS_POSITIVE(delta_Q));

	free(indices);
	free(score);
	free(improve);
	free_nodes(unmoved);

	return all_delta;
}

void free_nodes(node* nodes)
{
	node* curr_node = NULL;

	while(nodes != NULL)
	{
		curr_node = nodes;
		nodes = nodes->next;
		free(curr_node);
	}
}

void create_s (int* g, int* g1, elem* s, int size, int g1_size)
{
	int i = 0, g1_i = 0;

	for (i = 0; i < size; ++i)
	{
		if (g1_i < g1_size && g[i] == g1[g1_i])
		{
			s[i] = 1;
			g1_i++;
		}
		else
		{
			s[i] = -1;
		}
	}
}
